Class PoseGraph

Class Documentation

class PoseGraph

Handles reading/writing, loading/saving and optimization of key frames.

Public Functions

PoseGraph()

Pose graph class constructor.

Handles reading/writing, loading/saving and optimization of key frames.

Parameters

none. –

~PoseGraph()

Pose graph class destructor.

Parameters

none. –

void registerPub(ros::NodeHandle &n)

Get ready for the output path, pointcloud, odometry, visual marker in ROS format.

Parameters

ROS – [in] node handler passed from external ROS node main function.

void addKeyFrame(std::shared_ptr<KeyFrame> &cur_kf, bool flag_detect_loop)

Add observed new keyframe into the keyframe database.

Parameters

  • keyframe – [in] pointer. After KeyFrame() constructor, we get a shared_ptr of keyframe.

  • Flag – [in] of whether detecting a loop.

void loadKeyFrame(std::shared_ptr<KeyFrame> &cur_kf, bool flag_detect_loop)

Add loaded keyframe from previously stored prior map into the current keyframe database.

Parameters

  • keyframe – [in] pointer. Loaded “old” keyframe.

  • Flag – [in] of whether detecting a loop. (Normally set to “false”)

void loadVocabulary(std::string voc_path)

Load vocabulary of DBOW2 training samples for future loop recognition.

Parameters

Vocabulary – [in] file location.

void setIMUFlag(bool _use_imu)

Proceed optimization based on the type of VO source. VIO directly uses 4D and VO uses 6D.

Parameters

Flag – [in] of whether the VO source has inertial sensor.

inline void setTrajFlag(int display_traj)

Decide whether displaying the prior map trajectory for visualization.

Parameters

Flag – [in] of whether displaying the prior map trajectory.

std::shared_ptr<KeyFrame> getKeyFrame(int index)

Acquire the desired keyframe pointer according to the inquiry index.

Note

std::shared_ptr only supported by C++ 11 standard or above.

Parameters

Inquiry – [in] index (int).

Returns

the inquired keyframe pointer.

void savePoseGraph()

Save current pose graph into predefined location.

void loadPoseGraph()

Load prior pose graph from predefined location.

void publish()

Publish all the topics for visualization.

Note

Must run in a loop to keep updated.

Public Members

nav_msgs::Path path[10]

Trajectory of current path for visualization.

nav_msgs::Path base_path

Trajectory of prior path for visualization.

sensor_msgs::PointCloud base_point_cloud

Point cloud of pior map for visualization.

CameraPoseVisualization *posegraph_visualization

Camera model of current pose for visualization.

Vector3d t_drift

The translation drift from current keyframe to associated keyframe.

Note

It is zero if there is no loop.

double yaw_drift

The yaw angle from current keyframe to associated keyframe.

Matrix3d r_drift

The rotation drift from current keyframe to associated keyframe.

Note

It is identity if there is no loop.

Vector3d w_t_vio

The translation vector from initial pose to world fixed frame.

Note

It is zero if there is no prior map.

Matrix3d w_r_vio

The rotation matrix from initial pose to world frame.

Note

It is identity if there is no prior map.

bool load_gps_info

Whether load GPS initial alignment.

Vector3d gps_0_trans

The initial translation vector from current GPS position to ENU frame.

Quaterniond gps_0_q

The initial orientation matrix from current GPS position to ENU frame.

bool load_map

Whether load prior map.